#include "gimbal_client.h"

#include <unistd.h>

#include <chrono>
#include <cmath>
#include <cstdio>
#include <limits>
#include <thread>

#include "global.h"

uint16_t zoom_num[25] = {0x0000, 0x03D7, 0x0851, 0x0C28, 0x10A3, 0x147A, 0x18F5, 0x1CCC, 0x2036,
                         0x22C5, 0x25C2, 0x28BF, 0x2B17, 0x2C96, 0x2DDD, 0x2F5C, 0x30A3, 0x3222,
                         0x3369, 0x34E8, 0x3666, 0x3A74, 0x3C96, 0x3DF9, 0x3F5C};

// 水平垂直视场角
float hfov_arr[26] = {58.40, 29.90, 20.25, 15.28, 12.29, 10.25, 8.79,  7.70, 6.85,
                      6.17,  5.60,  5.13,  4.74,  4.40,  4.08,  3.83,  3.60, 3.39,
                      3.22,  3.04,  2.92,  2.77,  2.65,  2.52,  54.41, 31.7};
float vfov_arr[26] = {34.14, 17.22, 11.53, 8.66, 6.94, 5.79, 4.95,  4.35, 3.86,
                      3.46,  3.16,  2.88,  2.67, 2.47, 2.31, 2.15,  2.04, 1.91,
                      1.83,  1.72,  1.65,  1.57, 1.50, 1.42, 29.84, 24.0};

GimbalClient::GimbalClient() {}

GimbalClient::~GimbalClient() {}

size_t WriteCallback(void *contents, size_t size, size_t nmemb, std::string *s)
{
    size_t newLength = size * nmemb;
    try
    {
        s->append((char *)contents, newLength);
    }
    catch (std::bad_alloc &e)
    {
        return 0;
    }
    return newLength;
}

bool GimbalClient::send_target_to_box(uint8_t id, std::string alarm_type, uint16_t target_w,
                                      uint16_t target_h, uint8_t alarmlv, uint8_t block_class)
{
    time_t now_time = time(NULL);
    CURL *curl;
    CURLcode res;
    nlohmann::json send_json;
    std::string send_msg;
    curl = curl_easy_init();  // 初始化

    // custom_printf("curl init cost time:%ld", time(NULL)-now_time);
    struct tm *local;
    local = localtime(&now_time);

    char id_str[64] = {0};
    sprintf(id_str, "%d%d", id, now_time);

    char time_str[64] = {0};
    sprintf(time_str, "%d-%02d-%02d %02d:%02d:%02d", local->tm_year + 1900, local->tm_mon + 1,
            local->tm_mday, local->tm_hour, local->tm_min, local->tm_sec);

    float fov = hfov_arr[current_zoom] / 2 * PI / 180;
    float distance = 640 / target_w * 0.3 / tan(fov);
    int size = target_w * target_h;
    send_json["event_id"] = id_str;
    send_json["time"] = time_str;
    send_json["alarm_type"] = alarm_type.c_str();
    send_json["ptz_angle"]["pan"] = current_pan;
    send_json["ptz_angle"]["tilt"] = current_tilt;
    send_json["distance"] = distance;
    send_json["target_size"] = size;
    send_json["block_result"] = alarmlv;
    send_json["target_class"] = block_class;
    send_json["height"] = 10;

    send_msg = send_json.dump();
    std::string readBuffer;
    // std::cout<<"send_url:"<<"192.168.1.44:8848/hiface/alarm/AlarmEventInfo"<<"\nsend_json"<<send_json<<std::endl;

    struct curl_slist *header_list = NULL;
    header_list = curl_slist_append(header_list, "Content-Type:application/json");

    if (curl)
    {
        curl_easy_setopt(curl, CURLOPT_HTTPHEADER, header_list);
        curl_easy_setopt(curl, CURLOPT_URL, "192.168.1.44:8848/hiface/alarm/AlarmEventInfo");
        curl_easy_setopt(curl, CURLOPT_POST, 1L);
        curl_easy_setopt(curl, CURLOPT_POSTFIELDS, send_msg.c_str());

        // 设置回调函数，用于接收服务器返回的数据
        curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, WriteCallback);
        curl_easy_setopt(curl, CURLOPT_WRITEDATA, &readBuffer);

        // 设置连接超时时间为 200 毫秒
        curl_easy_setopt(curl, CURLOPT_CONNECTTIMEOUT_MS, 200);
        // 设置整个操作超时时间为 500 毫秒
        curl_easy_setopt(curl, CURLOPT_TIMEOUT_MS, 500);

        res = curl_easy_perform(curl);
        // custom_printf("curl_easy_perform cost time:%ld", time(NULL)-now_time);
        if (res != CURLE_OK)
        {
            curl_slist_free_all(header_list);
            curl_easy_cleanup(curl);
            return false;
        }
        return true;
    }
    return false;
}

bool GimbalClient::gimbal_init()
{
    screen_show_fd = socket(PF_INET, SOCK_DGRAM, 0);
    if (screen_show_fd == -1)
    {
        custom_printf("Create socket failed.\n");
        return false;
    }

    remote_addr.sin_family = AF_INET;
    remote_addr.sin_port = htons(18500);
    inet_pton(AF_INET, "127.0.0.1", &remote_addr.sin_addr.s_addr);

    local_addr.sin_family = AF_INET;
    local_addr.sin_port = htons(18501);
    local_addr.sin_addr.s_addr = INADDR_ANY;

    if (bind(screen_show_fd, (struct sockaddr *)&local_addr, sizeof(local_addr)) == -1)
    {
        custom_printf("Bind failed: %s\n", strerror(errno));
        close(screen_show_fd);
        return false;
    }

    if (connect(screen_show_fd, (struct sockaddr *)&remote_addr, sizeof(remote_addr)) == -1)
    {
        custom_printf("Connect failed: %s\n", strerror(errno));
        close(screen_show_fd);
        return false;
    }

    int flags = fcntl(screen_show_fd, F_GETFL, 0);
    if (flags == -1 || fcntl(screen_show_fd, F_SETFL, flags | O_NONBLOCK) == -1)
    {
        custom_printf("Set non-block failed: %s\n", strerror(errno));
        close(screen_show_fd);
        return false;
    }

    struct timeval timeout = {1, 0};
    if (setsockopt(screen_show_fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) == -1)
    {
        custom_printf("Setsockopt failed: %s\n", strerror(errno));
    }

    if (!ptserial_fd)
    {
        custom_printf("Invalid serial port device\n");
        close(screen_show_fd);
        return false;
    }

    custom_printf("Open serial port: %s", ptserial_fd);
    if (!ptserial.OpenPort(ptserial_fd, B19200))
    {
        custom_printf("Open serial port %s failed\n", ptserial_fd);
        close(screen_show_fd);
        return false;
    }

    if (system("bspmm 0x102600e0 0x1201 > /dev/null 2>&1") != 0)
    {
        custom_printf("System command failed\n");
    }

    start_thread();
    return true;
}

void GimbalClient::lpp_command(double pan_control, double tilt_control)
{
    constexpr int BUF_SIZE = 10;
    uint8_t buf[BUF_SIZE] = {0};

    // 包头和地址
    buf[0] = 0xa5;
    buf[1] = 0x01;

    // 方向控制
    std::string debug_msg;
    if (std::abs(pan_control) > std::numeric_limits<double>::epsilon()) {
        buf[2] |= (pan_control > 0) ? 0x40 : 0x80; // 0x40: 右转，0x80: 左转
    }
    if (std::abs(tilt_control) > std::numeric_limits<double>::epsilon()) {   
        buf[2] |= (tilt_control > 0) ? 0x20 : 0x10; // 0x20: 上升，0x10: 下降
    }

    // 时间参数
    // buf[5] = 0x14; // 200ms
    // buf[8] = 0x14; // 200ms

    buf[5] = 0x64;  // 1000ms
    buf[8] = 0x64;  // 1000ms

    // 速度计算和限制
    uint16_t pan_speed =
    std::min(static_cast<uint16_t>(fabs(pan_control) * 100), static_cast<uint16_t>(0xFFFF));
    uint16_t tilt_speed =
    std::min(static_cast<uint16_t>(fabs(tilt_control) * 100), static_cast<uint16_t>(0xFFFF));
    
    // 速度设置
    buf[3] = (pan_speed >> 8) & 0xFF;
    buf[4] = pan_speed & 0xFF;
    buf[6] = (tilt_speed >> 8) & 0xFF;
    buf[7] = tilt_speed & 0xFF;

    // custom_printf("Control: pan=%.2f tilt=%.2f", pan_control, tilt_control);
    // custom_printf("Packet: %s", bytesToHexString(buf, BUF_SIZE).c_str());
    // custom_printf("=> 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], buf[8], buf[9]);

    ptserial.writeBuffer(buf, sizeof(buf));

    std::cout << debug_msg << std::endl;
    // std::this_thread::sleep_for(std::chrono::milliseconds(100));
}

void GimbalClient::lpp_control_step(double target_pan, double target_tilt, double error_prescision, int current_zoom,bool use_initial_pid) {
    PIDController* current_pan_pid = use_initial_pid ? &pan_pid_initial : &pan_pid;
    PIDController* current_tilt_pid = use_initial_pid ? &tilt_pid_initial : &tilt_pid;
    // 步骤二，如果误差都小于 2 度，认为已经达到目标角度，输出提示信息并退出循环
    double pan_error = std::abs(current_pan - target_pan);
    if (pan_error > 180)
    {
        pan_error = 360 - pan_error;
    }
    double tilt_error = std::abs(current_tilt - target_tilt);
    if (tilt_error > 90)
    {
        tilt_error = 180 - tilt_error;
    }
    if (pan_error < error_prescision && tilt_error < error_prescision)
    {
        lpp_command(0.0, 0.0);
        return;
    }
    // 步骤三计算pid输出。使用方位角 PID 控制器计算控制输出，根据目标方位角和当前方位角进行计算
    

    // double pan_control = pan_pid.calculate(target_pan, current_pan, current_zoom);
    // double tilt_control = tilt_pid.calculate(target_tilt, current_tilt, current_zoom);
    // 步骤三计算pid输出。使用方位角 PID 控制器计算控制输出，根据目标方位角和当前方位角进行计算
    double pan_control = current_pan_pid->calculate(target_pan, current_pan, current_zoom);
    double tilt_control = current_tilt_pid->calculate(target_tilt, current_tilt, current_zoom);
    // custom_printf("Current Pan = %.2f, target Pan = %.2f, Current Tilt = %.2f, target Tilt = %.2f, pan control = %.2f, tilt control = %.2f, pan error = %.2f, tilt error = %.2f", current_pan, target_pan, current_tilt, target_tilt, pan_control, tilt_control, pan_error, tilt_error);
    // 步骤四， pid控制云台运动
    lpp_command(pan_control, tilt_control);
}


bool GimbalClient::lpp_control_to_destination(double target_pan, double target_tilt, int zoom, int step_count, double error_prescision) {
    // custom_printf("Start lpp_control_to_destination");
    
    for (int i = 0; i < step_count; ++i) {
        int pan_error = std::abs(current_pan - target_pan);
        if (pan_error > 180)
        {
            pan_error = 360 - pan_error;
        }
        int tilt_error = std::abs(current_tilt - target_tilt);
        if (tilt_error > 90)
        {
            tilt_error = 180 - tilt_error;
        }
        if (pan_error < error_prescision && tilt_error < error_prescision) {
            lpp_command(0.0, 0.0); // 停止命令
            return true;
        }
        // 步骤三计算pid输出。使用方位角 PID 控制器计算控制输出，根据目标方位角和当前方位角进行计算
        // double pan_control = pan_pid.calculate(target_pan, current_pan, zoom);
        // double tilt_control = tilt_pid.calculate(target_tilt, current_tilt, zoom);
        double pan_control = pan_pid_initial.calculate(target_pan, current_pan, zoom);
        double tilt_control = tilt_pid_initial.calculate(target_tilt, current_tilt, zoom);
        // 步骤四， pid控制云台运动
        // custom_printf("Current Pan = %.2f, target Pan = %.2f, Current Tilt = %.2f, target Tilt = %.2f, pan control = %.2f, tilt control = %.2f, pan error = %.2f, tilt error = %.2f",
            //  current_pan, target_pan, current_tilt, target_tilt, pan_control, tilt_control, pan_error, tilt_error);
        lpp_command(pan_control, tilt_control);
        set_zoom_ratio(zoom);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    return false;
}


void GimbalClient::lpp_control_step_pixel(bbox_t phase1_bbox,int& current_zoom) {
    static bool target_detected = false;
    // 获取变倍时间戳
    static time_t last_zoom_time = 0;
    time_t now = time(NULL);

    double dx = phase1_bbox.cx - 0.5;
    double dy = 0.5 - phase1_bbox.cy;

    
    
    // 变倍逻辑优化：增加滞后阈值和分阶段调整
    const int ZOOM_LOW_BASE = 160;    // 基础低阈值（变倍1级时的阈值）
    const int ZOOM_HIGH_BASE = 210;   // 基础高阈值（变倍1级时的阈值）
    static int last_zoom = current_zoom;  // 记录上次变倍值
    const int HYSTERESIS = 10;  // 变倍滞后区间
    int zoom_step = 1;

    double duration = difftime(now, last_zoom_time);
    if (phase1_bbox.w != 0 && duration >= 0.03) {
        if (phase1_bbox.w >= ZOOM_HIGH_BASE + HYSTERESIS) {
            // 目标过大，根据偏差调整步长（偏差超过50像素时调整2级）
            zoom_step = (phase1_bbox.w - ZOOM_HIGH_BASE) > 20 ? 2: 1;
            current_zoom = std::max(1, current_zoom - zoom_step);
            last_zoom_time = now;
        } else if (phase1_bbox.w < ZOOM_LOW_BASE - HYSTERESIS) {
            // 目标过小，根据偏差调整步长（偏差超过50像素时调整2级）
            zoom_step = (ZOOM_LOW_BASE - phase1_bbox.w) > 20 ? 2 : 1;
            current_zoom = std::min(25, current_zoom + zoom_step);
            last_zoom_time = now;
        }
        // 仅当倍率变化时发送变倍指令，避免重复发送
        if (current_zoom != last_zoom) {                 // todoo: 优化
            if (set_zoom_ratio(current_zoom))
            std::cout << "变倍成功，此时current_zoom = " << current_zoom << std::endl;
            last_zoom = current_zoom;
            std::this_thread::sleep_for(std::chrono::milliseconds(50)); // 变倍过渡时间
        } 
        // 更新目标检测状态
        target_detected = (phase1_bbox.w > 0);
    } 
    else if (phase1_bbox.w = 0){
        target_detected = false;
    }

    double target_pan = current_pan + dx * hfov_arr[current_zoom];  // 水平移动角度
    double target_tilt = current_tilt + dy * vfov_arr[current_zoom];  // 垂直移动角度
    lpp_control_step(target_pan, target_tilt, 2.0, current_zoom, !target_detected);

}

// 在水平视场角中数组中，找到与目标值最接近的元素的索引，根据索引确定对应的zoom倍率
int GimbalClient::findClosestIndex(const float nums[], int size, float target)
{
    int closestIndex = 0;  // 初始化最接近的元素的索引为数组的第一个元素的索引
    int index = 0;
    float minDiff = std::numeric_limits<float>::max();  // 初始化最小差值为float类型的最大值

    while (index < size)
    {
        // 计算当前mid与target的差值
        float diff = fabs(nums[index] - target);
        // 如果当前差值更小，则更新最接近的元素的索引和最小差值
        if (diff < minDiff)
        {
            minDiff = diff;
            closestIndex = index;
        }
        index++;
    }
    // std::cout << "closestIndex: " << closestIndex <
    // 0219< std::endl;
    return closestIndex;
}

bool GimbalClient::set_pan_absolute(float angle)
{
    if (angle < 0) angle = 0;
    if (angle > 360) angle = 360;
    uint16_t val = angle * 100;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x4B;
    buf[4] = (val >> 8) & 0xFF;
    buf[5] = (val >> 0) & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    current_pan = angle;
    return true;
}

bool GimbalClient::set_tilt_absolute(float angle)
{
    if (angle < -90) angle = -90;
    if (angle > 90) angle = 90;

    uint16_t val = 0;
    if (angle > 0)
    {
        val = (360 - angle) * 100;
    }
    else
    {
        val = abs(angle) * 100;
    }
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x4D;
    buf[4] = (val >> 8) & 0xFF;
    buf[5] = (val >> 0) & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    current_tilt = angle;
    return true;
}

bool GimbalClient::set_pan_absolute(float angle, uint8_t speed)
{
    if (angle < 0) angle = 0;
    if (angle > 360) angle = 360;
    uint16_t val = angle * 100;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = speed;
    buf[3] = 0x4B;
    buf[4] = (val >> 8) & 0xFF;
    buf[5] = (val >> 0) & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    current_pan = angle;
    return true;
}

bool GimbalClient::set_tilt_absolute(float angle, uint8_t speed)
{
    if (angle < -90) angle = -90;
    if (angle > 90) angle = 90;

    uint16_t val = 0;
    if (angle > 0)
    {
        val = (360 - angle) * 100;
    }
    else
    {
        val = abs(angle) * 100;
    }
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = speed;
    buf[3] = 0x4D;
    buf[4] = (val >> 8) & 0xFF;
    buf[5] = (val >> 0) & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    current_tilt = angle;
    return true;
}

// bool GimbalClient::stop_move(){
//     uint8_t buf[7]={0};
//     buf[0]=0xFF;
//     buf[1]=0x01;
//     buf[2]=0x00;
//     buf[3]=0x00;
//     buf[4]=0x00;
//     buf[5]=0x00;
//     buf[6]=buf[1]+buf[2]+buf[3]+buf[4]+buf[5];
//     ptserial.writeBuffer(buf,sizeof(buf));
//     return true;
// }

bool GimbalClient::set_countermeasure(bool state)
{
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[4] = 0x00;
    buf[5] = 0x03;
    if (state)
    {
        buf[3] = 0x09;
        update_screen_osd(1, 8);
    }
    else
    {
        buf[3] = 0x0B;
        update_screen_osd(1, 0);
    }
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::set_pan_relative(float angle)
{
    float abs_angle = current_pan + angle;

    if (abs_angle < 0) abs_angle += 360;
    if (abs_angle > 360) abs_angle -= 360;

    uint16_t val = abs_angle * 100;
    uint8_t buf[7] = {0};

    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x4B;
    buf[4] = (val >> 8) & 0xFF;
    buf[5] = (val >> 0) & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));

    return true;
}

bool GimbalClient::set_tilt_reltive(float angle)
{
    float abs_angle = current_tilt + angle;
    if (abs_angle > 90) abs_angle = 90;

    uint16_t val = 0;
    if (abs_angle > 0)
    {
        val = (360 - abs_angle) * 100;
    }
    else
    {
        val = abs(abs_angle) * 100;
    }
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x4D;
    buf[4] = (val >> 8) & 0xFF;
    buf[5] = (val >> 0) & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::set_pan_relative(float angle, uint8_t speed)
{
    float abs_angle = current_pan + angle;

    if (abs_angle < 0) abs_angle += 360;
    if (abs_angle > 360) abs_angle -= 360;

    uint16_t val = abs_angle * 100;
    uint8_t buf[7] = {0};

    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = speed;
    buf[3] = 0x4B;
    buf[4] = (val >> 8) & 0xFF;
    buf[5] = (val >> 0) & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));

    return true;
}

bool GimbalClient::set_tilt_relative(float angle, uint8_t speed)
{
    float abs_angle = current_tilt + angle;

    if (abs_angle < -90) abs_angle = -90;
    if (abs_angle > 90) abs_angle = 90;

    uint16_t val = 0;
    if (abs_angle > 0)
    {
        val = (360 - abs_angle) * 100;
    }
    else
    {
        val = abs(abs_angle) * 100;
    }
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = speed;
    buf[3] = 0x4D;
    buf[4] = (val >> 8) & 0xFF;
    buf[5] = (val >> 0) & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

float GimbalClient::get_current_pan() { return current_pan; }

float GimbalClient::get_current_tilt() { return current_tilt; }

bool GimbalClient::update_pt_absolute()
{
    const int wp_zoom = 1;   // 反制和倍率
    const int pan_tilt = 2;  // 云台pt
    update_screen_osd(wp_zoom);
    while (true)
    {
        uint8_t buf[14] = {0};
        int nrecv = ptserial.readBuffer(buf, sizeof(buf));
        if (nrecv == 14)
        {
            uint8_t sum1 = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
            uint8_t sum2 = buf[8] + buf[9] + buf[10] + buf[11] + buf[12];

            if (buf[6] != sum1 || buf[13] != sum2)
            {
                custom_printf("Sum error %d %d %d %d\n", buf[6], sum1, buf[13], sum2);
                continue;
            }
            int pan_data = buf[4] * 256 + buf[5];
            current_pan = pan_data / 100.0;
            int tilt_data = buf[11] * 256 + buf[12];
            if (tilt_data >= 18000)
            {
                tilt_data = 36000 - tilt_data;
            }
            else
            {
                tilt_data = -tilt_data;
            }
            current_tilt = tilt_data / 100.0;
            // custom_printf("Update Pan: %.2f, Tilt: %.2f", current_pan,
            // current_tilt);
            update_screen_osd(pan_tilt);
        }
        // else {
        //     custom_printf("Read buffer error %d\n", nrecv);
        // }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    return true;
}

void GimbalClient::start_thread()
{
    custom_printf("Gimbal thread started ...");
    std::thread pt_thread(&GimbalClient::update_pt_absolute, this);
    pt_thread.detach();
}

bool GimbalClient::stop_move()
{
    uint8_t buf[7] = {0xFF, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01};
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::move_up(uint8_t speed)
{
    uint8_t speed_F = speed * 0.64;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x08;
    buf[4] = 0x00;
    buf[5] = speed_F & 0xFF;
    // buf[5]=0x00;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::move_left(uint8_t speed)
{
    uint8_t speed_F = speed * 0.64;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x04;
    buf[4] = speed_F & 0xFF;
    buf[5] = 0x00;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::move_right(uint8_t speed)
{
    uint8_t speed_F = speed * 0.64;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x02;
    buf[4] = speed_F & 0xFF;
    buf[5] = 0x00;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::move_down(uint8_t speed)
{
    uint8_t speed_F = speed * 0.64;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x10;
    buf[4] = 0x00;
    buf[5] = speed_F & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::move_left_top(uint8_t speed)
{
    uint8_t speed_F = speed * 0.64;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x0C;
    buf[4] = speed_F & 0xFF;
    buf[5] = speed_F & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::move_right_top(uint8_t speed)
{
    uint8_t speed_F = speed * 0.64;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x0A;
    buf[4] = speed_F & 0xFF;
    buf[5] = speed_F & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::move_left_down(uint8_t speed)
{
    uint8_t speed_F = speed * 0.64;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x14;
    buf[4] = speed_F & 0xFF;
    buf[5] = speed_F & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::move_right_down(uint8_t speed)
{
    uint8_t speed_F = speed * 0.64;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x12;
    buf[4] = speed_F & 0xFF;
    buf[5] = speed_F & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}



bool GimbalClient::set_preset_point(uint8_t point)
{
    std::cout << "设置预制点位" << std::endl;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x03;
    buf[4] = 0x00;
    buf[5] = point & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::move_to_preset_point(uint8_t point)
{
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x07;
    buf[4] = 0x00;
    buf[5] = point & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    return true;
}

bool GimbalClient::set_zoom_ratio(uint8_t ratio)
{
    current_zoom = ratio;
    uint8_t buf[7] = {0};
    buf[0] = 0xFF;
    buf[1] = 0x01;
    buf[2] = 0x00;
    buf[3] = 0x4f;
    buf[4] = (zoom_num[ratio - 1] >> 8) & 0xFF;
    buf[5] = (zoom_num[ratio - 1] >> 0) & 0xFF;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));
    update_screen_osd(1);
    return true;
}

bool GimbalClient::add_zoom_ratio()
{
    const uint8_t MAX_ZOOM = 25;
    if (current_zoom >= MAX_ZOOM)
    {
        custom_printf("Reached max zoom ratio\n");
        return false;
    }
    current_zoom++;
    return set_zoom_ratio(current_zoom);
}

bool GimbalClient::sub_zoom_ratio()
{
    const uint8_t MIN_ZOOM = 1;
    if (current_zoom <= MIN_ZOOM)
    {
        current_zoom = MIN_ZOOM;
        custom_printf("Reached min zoom ratio\n");
        return false;
    }
    current_zoom--;
    return set_zoom_ratio(current_zoom);
}

uint8_t GimbalClient::get_current_zoom() { return current_zoom; }

bool GimbalClient::set_counter_action(uint8_t counter_action, bool state)
{
    if (state)
    {
        counter_action_ |= counter_action;
    }
    else
    {
        counter_action_ &= ~counter_action;
    }
    return 1;
}

bool GimbalClient::update_screen_osd(uint8_t update_osd_type, uint8_t wp)
{
    char buf[8] = {0};
    buf[0] = 0xAA;
    buf[1] = 0x55;
    buf[2] = update_osd_type;
    buf[7] = 0xFF;

    switch (update_osd_type)
    {
        case 1:  // 发送反制和倍率
            buf[3] = wp;
            buf[4] = current_zoom;
            break;

        case 2:  // 发送pan和title角度
            float tilt = current_tilt;
            uint16_t num_int = (int)current_pan;
            uint16_t num_fra = (int)((current_pan - num_int) * 10);
            uint32_t num = num_int << 4 | num_fra;

            buf[3] = num >> 8;
            buf[4] = num & 0xFF;

            if (tilt < 0)
            {
                tilt = -tilt;
                buf[6] |= 0x10;
            }

            num_int = (int)tilt;
            num_fra = (int)((tilt - num_int) * 10);

            buf[5] = num_int & 0xFF;
            buf[6] |= num_fra;
            break;
    }
    sendto(screen_show_fd, buf, sizeof(buf), 0, (struct sockaddr *)&remote_addr,
           sizeof(remote_addr));
    return true;
}

bool GimbalClient::IR_switch(bool state)
{
    uint8_t buf[7] = {0};

    buf[0] = 0xFF;
    buf[1] = 0x02;
    buf[2] = 0x01;
    buf[3] = 0x01;
    buf[5] = 0x00;

    if (state)
    {
        buf[4] = 0x01;
        buf[6] = 0x05;
    }
    else
    {
        buf[4] = 0x00;
        buf[6] = 0x04;
    }

    ptserial.writeBuffer(buf, sizeof(buf));

    return 1;
}

bool GimbalClient::setIRlm(uint8_t lm)
{
    uint8_t buf[7] = {0};

    buf[0] = 0xFF;
    buf[1] = 0x02;
    buf[2] = 0x01;
    buf[3] = 0x03;
    buf[4] = lm;
    buf[5] = 0x00;
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
    ptserial.writeBuffer(buf, sizeof(buf));

    return 1;
}

bool GimbalClient::setIRzoom(bool state, uint8_t ratio)
{
    uint8_t buf[7] = {0};

    buf[0] = 0xFF;
    buf[1] = 0x02;
    buf[2] = 0x01;
    buf[3] = 0x04;
    buf[5] = ratio;

    if (state)
    {
        buf[4] = 0x01;
    }
    else
    {
        buf[4] = 0x00;
    }
    buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];

    ptserial.writeBuffer(buf, sizeof(buf));

    return 1;
}

bool GimbalClient::requset_record()
{
    CURL *curl;
    CURLcode res;
    curl = curl_easy_init();  // 初始化

    if (curl)
    {
        curl_easy_setopt(curl, CURLOPT_URL, "192.168.1.44:8848/alarm_hiface");
        res = curl_easy_perform(curl);  // 执行
        if (res != 0)
        {
            curl_easy_cleanup(curl);
        }
        return true;
    }
    return false;
}

static uint16_t ModBusCRC16(unsigned char *data, unsigned int len)
{
    unsigned int i, j, tmp, CRC16;

    CRC16 = 0xFFFF;  // CRC寄存器初始值
    for (i = 0; i < len; i++)
    {
        CRC16 ^= data[i];
        for (j = 0; j < 8; j++)
        {
            tmp = (unsigned int)(CRC16 & 0x0001);
            CRC16 >>= 1;
            if (tmp == 1)
            {
                CRC16 ^= 0xA001;  // 异或多项式
            }
        }
    }

    return CRC16;
}

bool GimbalClient::ptz_update(char *update_path)
{
    gimbal_update = true;
    // 跳转升级指令
    set_preset_point(239);
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    std::cout << "设置波特率" << std::endl;
    ptserial.setbaudrate(B115200);
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    // 连接bootloade程序
    std::cout << "连接bootloade程序" << std::endl;  // 6F 05 FF 02 CD
    uint8_t test_progream_buf[5] = {0x7F, 0x05, 0x01, 0x82, 0x88};
    ptserial.writeBuffer(test_progream_buf, sizeof(test_progream_buf));
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    int nrecv = ptserial.readBuffer(test_progream_buf, sizeof(test_progream_buf));
    if (nrecv >= 5)
    {
        if (test_progream_buf[3] != 0x02 || test_progream_buf[4] != 0xCD) return false;
    }
    else
    {
        return false;
    }

    std::ifstream updatefile(update_path, std::ios::binary);
    if (!updatefile)
    {
        std::cout << "无法打开文件" << std::endl;
        return false;
    }
    updatefile.seekg(0, std::ios::end);
    std::streamsize read_fileSize = updatefile.tellg();
    updatefile.seekg(0, std::ios::beg);
    uint64_t filesize = read_fileSize;

    // 发送升级文件信息
    std::cout << "发送升级文件信息" << std::endl;  // 6F 01 01 81 8D
    uint8_t start_up_buf[12] = {0x7F, 0x01, 0x29, 0x00, 0x00, 0x00,
                                0x7c, 0xA2, 0x00, 0x00, 0x00, 0x00};
    uint64_t send_count = filesize / 1024;

    if (filesize % 1024 != 0)
    {
        send_count += 1;
    }

    start_up_buf[2] = send_count & 0xFF;
    start_up_buf[3] = (send_count >> 8) & 0xFF;
    start_up_buf[4] = (send_count >> 16) & 0xFF;
    start_up_buf[5] = (send_count >> 24) & 0xFF;

    start_up_buf[6] = filesize & 0xFF;
    start_up_buf[7] = (filesize >> 8) & 0xFF;
    start_up_buf[8] = (filesize >> 16) & 0xFF;
    start_up_buf[9] = (filesize >> 24) & 0xFF;

    uint16_t CRC = ModBusCRC16(start_up_buf, 10);
    start_up_buf[10] = CRC & 0xFF;
    start_up_buf[11] = (CRC >> 8) & 0xFF;

    ptserial.writeBuffer(start_up_buf, sizeof(start_up_buf));
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    uint8_t recv_buf[5];
    nrecv = ptserial.readBuffer(recv_buf, sizeof(recv_buf));
    if (nrecv >= 5)
    {
        if (recv_buf[3] != 0x81 || recv_buf[4] != 0x8D)
        {
            updatefile.close();
            return false;
        }
    }
    else
    {
        updatefile.close();
        return false;
    }

    std::cout << "开始发送升级包,共计 " << send_count << " 个包" << std::endl;  // 6F 02 01 81 7D
    for (uint64_t i = 0; i < send_count; i++)
    {
        std::cout << "发送第 " << i + 1 << " 升级文件数据包" << std::endl;
        uint8_t *send_buf;
        uint64_t send_size;
        uint8_t head[10] = {0x7F, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00};
        head[2] = (i + 1) & 0xFF;
        head[3] = ((i + 1) >> 8) & 0xFF;
        head[4] = ((i + 1) >> 16) & 0xFF;
        head[5] = ((i + 1) >> 24) & 0xFF;

        if (i == send_count - 1)
        {
            uint16_t last_file_size = filesize % 1024;
            send_size = last_file_size + 12;
            send_buf = new uint8_t[last_file_size + 12];
            memset(send_buf, 0, last_file_size + 12);
            head[6] = last_file_size & 0xFF;
            head[7] = (last_file_size >> 8) & 0xFF;

            updatefile.read((char *)send_buf + 10, last_file_size);
        }
        else
        {
            send_buf = new uint8_t[1024 + 12];
            send_size = 1024 + 12;
            memset(send_buf, 0, 1024 + 12);

            updatefile.read((char *)send_buf + 10, 1024);
        }
        memcpy(send_buf, head, 10);

        CRC = ModBusCRC16(send_buf, send_size - 2);
        send_buf[send_size - 2] = CRC & 0xFF;
        send_buf[send_size - 1] = (CRC >> 8) & 0xFF;
        ptserial.writeBuffer(send_buf, send_size);

        if (i == 0)
            std::this_thread::sleep_for(std::chrono::milliseconds(2000));
        else
            std::this_thread::sleep_for(std::chrono::milliseconds(100));

        free(send_buf);

        int nrecv = ptserial.readBuffer(recv_buf, sizeof(recv_buf));
        if (nrecv >= 5)
        {
            if (recv_buf[3] != 0x81 || recv_buf[4] != 0x7D)
            {
                updatefile.close();
                return false;
            }
        }
        else
        {
            updatefile.close();
            return false;
        }
    }
    updatefile.close();

    // 结束升级
    std::cout << "结束升级" << std::endl;  // 6F 04 01 82 DD          ffffff4040
    uint8_t end_update_buf[5] = {0x7F, 0x04, 0x01, 0x82, 0xDD};
    ptserial.writeBuffer(end_update_buf, sizeof(end_update_buf));
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    nrecv = ptserial.readBuffer(end_update_buf, sizeof(end_update_buf));
    if (nrecv >= 5)
    {
        if (end_update_buf[3] != 0x40 || end_update_buf[4] != 0x40) return false;
    }
    else
    {
        return false;
    }

    // 云台程序重启
    std::cout << "云台程序重启" << std::endl;
    uint8_t restart_ptz_buf[6] = {0x7F, 0x03, 0x01, 0x01, 0x29, 0xA0};
    ptserial.writeBuffer(restart_ptz_buf, sizeof(restart_ptz_buf));
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    updatefile.close();
    ptserial.setbaudrate(B19200);
    gimbal_update = false;
    return true;
}

bool GimbalClient::ptz_update_485(char *update_path)
{
    gimbal_update = true;
    // 跳转升级指令
    set_preset_point(239);
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    std::cout << "设置波特率" << std::endl;
    ptserial.setbaudrate(B115200);
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    // 连接bootloade程序
    std::cout << "连接bootloade程序" << std::endl;  // 6F 05 FF 02 CD
    uint8_t test_progream_buf[5] = {0x7F, 0x05, 0x01, 0x82, 0x88};
    ptserial.writeBuffer_rs485(test_progream_buf, sizeof(test_progream_buf));
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    int nrecv = ptserial.readBuffer(test_progream_buf, sizeof(test_progream_buf));
    if (nrecv >= 5)
    {
        if (test_progream_buf[3] != 0x02 || test_progream_buf[4] != 0xCD) return false;
    }
    else
    {
        return false;
    }

    std::ifstream updatefile(update_path, std::ios::binary);
    if (!updatefile)
    {
        std::cout << "无法打开文件" << std::endl;
        return false;
    }
    updatefile.seekg(0, std::ios::end);
    std::streamsize read_fileSize = updatefile.tellg();
    updatefile.seekg(0, std::ios::beg);
    uint64_t filesize = read_fileSize;

    // 发送升级文件信息
    std::cout << "发送升级文件信息" << std::endl;  // 6F 01 01 81 8D
    uint8_t start_up_buf[12] = {0x7F, 0x01, 0x29, 0x00, 0x00, 0x00,
                                0x7c, 0xA2, 0x00, 0x00, 0x00, 0x00};
    uint64_t send_count = filesize / 1024;

    if (filesize % 1024 != 0)
    {
        send_count += 1;
    }

    start_up_buf[2] = send_count & 0xFF;
    start_up_buf[3] = (send_count >> 8) & 0xFF;
    start_up_buf[4] = (send_count >> 16) & 0xFF;
    start_up_buf[5] = (send_count >> 24) & 0xFF;

    start_up_buf[6] = filesize & 0xFF;
    start_up_buf[7] = (filesize >> 8) & 0xFF;
    start_up_buf[8] = (filesize >> 16) & 0xFF;
    start_up_buf[9] = (filesize >> 24) & 0xFF;

    uint16_t CRC = ModBusCRC16(start_up_buf, 10);
    start_up_buf[10] = CRC & 0xFF;
    start_up_buf[11] = (CRC >> 8) & 0xFF;

    ptserial.writeBuffer_rs485(start_up_buf, sizeof(start_up_buf));
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    uint8_t recv_buf[5];
    nrecv = ptserial.readBuffer(recv_buf, sizeof(recv_buf));
    if (nrecv >= 5)
    {
        if (recv_buf[3] != 0x81 || recv_buf[4] != 0x8D)
        {
            updatefile.close();
            return false;
        }
    }
    else
    {
        updatefile.close();
        return false;
    }

    std::cout << "开始发送升级包,共计 " << send_count << " 个包" << std::endl;  // 6F 02 01 81 7D
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    for (uint64_t i = 0; i < send_count; i++)
    {
        std::cout << "发送第 " << i + 1 << " 升级文件数据包" << std::endl;
        uint8_t *send_buf;
        uint64_t send_size;
        uint8_t head[10] = {0x7F, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00};
        head[2] = (i + 1) & 0xFF;
        head[3] = ((i + 1) >> 8) & 0xFF;
        head[4] = ((i + 1) >> 16) & 0xFF;
        head[5] = ((i + 1) >> 24) & 0xFF;

        if (i == send_count - 1)
        {
            uint16_t last_file_size = filesize % 1024;
            send_size = last_file_size + 12;
            send_buf = new uint8_t[last_file_size + 12];
            memset(send_buf, 0, last_file_size + 12);
            head[6] = last_file_size & 0xFF;
            head[7] = (last_file_size >> 8) & 0xFF;

            updatefile.read((char *)send_buf + 10, last_file_size);
        }
        else
        {
            send_buf = new uint8_t[1024 + 12];
            send_size = 1024 + 12;
            memset(send_buf, 0, 1024 + 12);

            updatefile.read((char *)send_buf + 10, 1024);
        }
        memcpy(send_buf, head, 10);

        CRC = ModBusCRC16(send_buf, send_size - 2);
        send_buf[send_size - 2] = CRC & 0xFF;
        send_buf[send_size - 1] = (CRC >> 8) & 0xFF;
        ptserial.writeBuffer_rs485(send_buf, send_size);
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
        free(send_buf);

        int nrecv = ptserial.readBuffer(recv_buf, sizeof(recv_buf));
        if (nrecv >= 5)
        {
            if (recv_buf[3] != 0x81 || recv_buf[4] != 0x7D)
            {
                updatefile.close();
                return false;
            }
        }
        else
        {
            updatefile.close();
            return false;
        }
    }
    updatefile.close();

    // 结束升级
    std::cout << "结束升级" << std::endl;  // 6F 04 01 82 DD
    uint8_t end_update_buf[5] = {0x7F, 0x04, 0x01, 0x82, 0xDD};

    ptserial.writeBuffer_rs485(end_update_buf, sizeof(end_update_buf));
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    nrecv = ptserial.readBuffer(end_update_buf, sizeof(end_update_buf));
    if (nrecv >= 5)
    {
        if (end_update_buf[3] != 0x82 || end_update_buf[4] != 0xDD) return false;
    }
    else
    {
        return false;
    }

    // 云台程序重启
    std::cout << "云台程序重启" << std::endl;

    uint8_t restart_ptz_buf[6] = {0x7F, 0x03, 0x01, 0x01, 0x29, 0xA0};
    ptserial.writeBuffer_rs485(restart_ptz_buf, sizeof(restart_ptz_buf));
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    updatefile.close();
    ptserial.setbaudrate(B19200);
    gimbal_update = false;
    return true;
}